/*
 * Ray Casting CPU
 *
 */

//#include "rayCastingCPU.h"
#include "Camera.h"
#include "Volume.h"
#include "FileManager.h"
#include "png.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>


#if 0
typedef struct
{
        float * startPoint;
        float * direction;
} Ray_t;

int cross(float *  vector1, float * vector2, float * result)
{
	//calculate the i,j,k coefficients of the cross product 
	result[0] = vector1[1]*vector2[2]-vector2[1]*vector1[2];
	result[1] = vector1[0]*vector2[2]-vector2[0]*vector1[2];
	result[2] = vector1[0]*vector2[1]-vector2[0]*vector1[1];

	return 0;
}


int ConstructRayThroughPixel(Ray_t * r, Camera_t * camera, int i, int j, int H, int W)
{
#if 0
	int	k	= 0;
	float * Du	= (float*) malloc(3*sizeof(float));
	float * Dv	= (float*) malloc(3*sizeof(float));
	float * ln  	= (float*) malloc(3*sizeof(float));
	float * dir  	= (float*) malloc(3*sizeof(float));
	//Du  = normalize(look x up)
	cross(camera->look, camera->up, Du);
	normalizeVector3D(Du);
	// Dv = normalize(look x Du)
	cross(camera->look, Du, Dv);
	normalizeVector3D(Dv);

	float fl = (float) (W / (2*tanf((0.5*camera->fov)*M_PI/180)));
	for(k=0;k<3;k++)
		ln[k] = camera->look[k];

	normalizeVector3D(ln);

	ln[0] = ln[0]*fl - 0.5*(W*Du[0]+H*Dv[0]);
	ln[1] = ln[1]*fl - 0.5*(W*Du[1]+H*Dv[1]);
	ln[2] = ln[2]*fl - 0.5*(W*Du[2]+H*Dv[2]);

	dir[0] = i*Du[0] + j*Dv[0] + ln[0];
	dir[1] = i*Du[1] + j*Dv[1] + ln[1];
	dir[2] = i*Du[2] + j*Dv[2] + ln[2];

	normalizeVector3D(dir);

	// Creamos Rayo
	for(k=0;k<3;k++)
	{
		r->startPoint[k]	= camera->position[k];
		r->direction[k] 	= dir[k];
	//	r->direction[k]		= r->startPoint[k] + ((i+0.5)/H)*camera->H*camera->up[k] + ((j+0.5)/W)*camera->W*camera->right[k]; 
	}
	free(Du);
	free(Dv);
	free(ln);
	free(dir);
#endif
#if 1
	int     k       = 0;

	for(k=0;k<3;k++)
        {
		r->startPoint[k]        = camera->position[k];
		r->direction[k]         = r->startPoint[k] + ((i+0.5)/H)*camera->H*camera->up[k] + ((j+0.5)/W)*camera->W*camera->right[k];
	}
	normalizeVector3D(r->direction);
#endif
	return 0;
}

int avanzar(Ray_t * r, float d, float * v)
{
        int i = 0;

        for(i=0; i<3; i++)
        {
                v[i] = r->startPoint[i]+(r->direction[i]*d);
        }

        return 0;
}

float tocado(float * p, volume_t * volume)
{
	float x0 = volume->position[0];
	float x1 = volume->position[0] + volume->dim[0];
	float y0 = volume->position[1];
	float y1 = volume->position[1] + volume->dim[1];
	float z0 = volume->position[2];
	float z1 = volume->position[2] + volume->dim[2];

	if ((p[0] > x0) && (p[0] < x1) && (p[1] > y0) && (p[1] < y1) && (p[2] > z0) && (p[2] < z1))
		return volume->data[(int)(p[0]-x0)][(int)(p[1]-y0)][(int)(p[2]-z0)];
	else
		return 0.0;

}



int FindIntersection(Ray_t ray, volume_t * volume, int * steps, float * px){

	int i = 0;
	float * p = (float*) malloc(3*sizeof(float));
	float hit = 0.0;
	
	for(i=0; i<10000 ; i++)
	{
		avanzar(&ray, (float)i, p);
		hit = tocado(p,volume);
		if (hit != 0.0)
		{
			p[0]*=hit;
			p[1]*=hit;
			p[2]*=hit;
			normalizeVector3D(p);
			px[0]+=p[0];
			px[1]+=p[1];
			px[2]+=p[2];
			
		}
	}
	free(p);

	return 0;

}


int RayCasting(Camera_t camera, volume_t * volume, unsigned char * screen, int width, int height)
{

	int 	i 	= 0;
	int 	j     	= 0;
	int 	k 	= 0;
	int 	steps 	= 0;
	float 	hit	= 0.0;

	Ray_t * r 	= (Ray_t*) malloc(sizeof(Ray_t)); 
	r->startPoint 	= (float*) malloc(3*sizeof(float));
	r->direction  	= (float*) malloc(3*sizeof(float));
	float * color 	= (float*) malloc(3*sizeof(float));

	for (i = 0; i < height; i++)
	{
		for (j = 0; j < width; j++)
		{
			ConstructRayThroughPixel(r,&camera, i, j, height, width);
			screen[k]    = -1;
			screen[k +1] = -1;
                        screen[k +2] = -1;
                        screen[k+3]  = 255;
			color[0]     = 0.0;
			color[1]     = 0.0;
			color[2]     = 0.0;
			FindIntersection(*r, volume,&steps,color);
			screen[k]    = color[0]*255;
			screen[k +1] = color[1]*255;
                        screen[k +2] = color[2]*255;
			fprintf(stdout, "(%u,%u,%u)   \n",screen[k],screen[k +1],screen[k +2]);
			k+=4;
		}
	}

	toPNG(screen,width,height);	

	free(r->startPoint);
	free(r->direction);
	free(r);
	

	return 0;
}




int main(int argc, char ** argv)
{
	int h = 200;
	int w = 200;

	unsigned char * s = (unsigned char *) malloc(4*h*w*sizeof(unsigned char));;
	volume_t v;
	float pos[3] = {-100.0, 20.0, -20.0};
	readHDF5(argv[1], "value", &v);
	setPositionVolume(&v, pos);

        float x0 = v.position[0];
        float x1 = v.position[0] + v.dim[0];
        float y0 = v.position[1];
        float y1 = v.position[1] + v.dim[1];
        float z0 = v.position[2];
        float z1 = v.position[2] + v.dim[2];
	fprintf(stdout,"Volumen (%f,%f,%f) (%f,%f,%f)\n",x0,y0,z0,x1,y1,z1);

	fprintf(stdout, "Volumen leido\n");

	Camera_t c;
	c.look 		= (float*) malloc(3*sizeof(float));
	c.up		= (float*) malloc(3*sizeof(float));
	c.position	= (float*) malloc(3*sizeof(float));
	c.right		= (float*) malloc(3*sizeof(float));
        c.distance	= 500.0;	

	c.look[0] 	= 1.0;
	c.look[1] 	= 0.0;
	c.look[2] 	= 0.0;
	c.up[0] 	= 0.0;
	c.up[1] 	= 1.0;
	c.up[2] 	= 0.0;
	c.right[0] 	= 0.0;
	c.right[1] 	= 0.0;
	c.right[2] 	= -1.0;
	c.position[0] 	= 0.0;
	c.position[1] 	= 0.0;
	c.position[2] 	= 0.0;
	c.fov		= M_PI/4; //45º atan(c.H/(2.0*c.distance));
	c.H		= 2.0*c.distance*tan(c.fov);
	c.W		= 2.0*c.distance*tan(c.fov);
	
	fprintf(stdout, "Angluo %f h %f w %f\n",c.fov,c.H,c.W);
	
	RayCasting(c, &v, s, w,h);

	return 0;
}
#endif

int normalizeVector3D(float * e)
{
	float magnitude = sqrtf(e[0] * e[0] + e[1] * e[1] + e[2] * e[2]);

	e[0] /= magnitude;
	e[1] /= magnitude;
	e[2] /= magnitude;

	return 0;
}

int cross(float *  vector1, float * vector2, float * result)
{
	//calculate the i,j,k coefficients of the cross product 
	float x = vector1[1]*vector2[2]-vector2[1]*vector1[2];
	float y = vector1[0]*vector2[2]-vector2[0]*vector1[2];
	float z = vector1[0]*vector2[1]-vector2[0]*vector1[1];
	result[0] = x;
	result[1] = y;
	result[2] = z;

	return 0;
}


float dot(float * v1, float * v2)
{
        return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}


int matrixVector3D(float * matrix, float * v, float * vo)
{
        float x = matrix[0] * v[0] + matrix[1] * v[1] + matrix[2]  * v[2];
        float y = matrix[4] * v[0] + matrix[5] * v[1] + matrix[6]  * v[2];
        float z = matrix[8] * v[0] + matrix[9] * v[1] + matrix[10] * v[2];

	vo[0]=x;
	vo[1]=y;
	vo[2]=z;

	return 0;
} 

int matrixXmatrix4D(float * m1, float * m2, float * m)
{
	int i = 0;
	int j = 0;
	
	for(i=0;i<4;i++)
	   for(j=0;j<4;j++)
		m[i*4+j] = m1[i*4+j]*m2[j*4+i];
	
	return 0;
}

int inverseMatrix(float *m, float * result)
{

	float * translation 	= (float*) malloc(3*sizeof(float));
	float * xAxis		= (float*) malloc(3*sizeof(float));
	float * yAxis		= (float*) malloc(3*sizeof(float));
	float * zAxis		= (float*) malloc(3*sizeof(float));
	translation[0]	= m[3];	translation[1]	= m[7]; 	translation[2]	= m[11];
	xAxis[0]	= m[0]; xAxis[1]	= m[4]; 	xAxis[2]	= m[8];
	yAxis[0]	= m[1]; yAxis[1]	= m[5]; 	yAxis[2]	= m[9];
	zAxis[0]	= m[2]; zAxis[1]	= m[6]; 	zAxis[2]	= m[10];

        result[0] = xAxis[0];   result[1] = xAxis[1];   result[2]  = xAxis[2];          result[3] = (-1.0)*dot(xAxis,translation);
        result[4] = yAxis[0];   result[5] = yAxis[1];   result[6]  = yAxis[2];          result[7] = (-1.0)*dot(yAxis,translation);
        result[8] = zAxis[0];   result[9] = zAxis[1];   result[10] = zAxis[2];          result[11] = (-1.0)*dot(zAxis,translation);
        result[12] = 0.0f;       result[13] = 0.0f;       result[14] = 0.0f;            result[15] = 1.0f;

	free(translation);
	free(xAxis);
	free(yAxis);
	free(zAxis);

        return 0;
}


float Max(float * t1)
{
	if (t1[0] > t1[1])
		return t1[0]>t1[2] ? t1[0] : t1[2];
	else
		return t1[1]>t1[2] ? t1[1] : t1[2];
		
}

float Min(float * t1)
{
	if (t1[0] < t1[1])
		return t1[0]<t1[2] ? t1[0] : t1[2];
	else
		return t1[1]<t1[2] ? t1[1] : t1[2];
}


void Swap(float* t1, float* t2)
{
        float t = *t2;
        *t2 = *t1;
        *t1 = t;
}

void SwapGR(float * t1, float * t2)
{
        if (t1[0] > t2[0])
        {
                Swap(&t1[0], &t2[0]);
        }

        if (t1[1] > t2[1])
        {
                Swap(&t1[1], &t2[1]);
        }

        if (t1[2] > t2[2])
        {
                Swap(&t1[2], &t2[2]);
        }
}

int avanzar(float * o, float * dir, float d, float * v)
{
        int i = 0;

        for(i=0; i<3; i++)
        {
                v[i] = o[i]+(dir[i]*d);
        }

        return 0;
}

int IntersectRayAABB(float * p, float * d, float * abboxMin, float * abboxMax, float * tmin, float * tmax)
{

	float * ood = (float *) malloc(3*sizeof(float));
	float * t1  = (float *) malloc(3*sizeof(float));
	float * t2  = (float *) malloc(3*sizeof(float));
	ood[0] = 1.0/d[0];
	ood[1] = 1.0/d[1];
	ood[2] = 1.0/d[2];

	t1[0] = (abboxMin[0] - p[0])*ood[0];
	t1[1] = (abboxMin[1] - p[1])*ood[1];
	t1[2] = (abboxMin[2] - p[2])*ood[2];
	t2[0] = (abboxMax[0] - p[0])*ood[0];
	t2[1] = (abboxMax[1] - p[1])*ood[1];
	t2[2] = (abboxMax[2] - p[0])*ood[2];
	

        SwapGR(t1, t2);

        *tmin = Max(t1);
        *tmax = Min(t2);

	free(ood);
	free(t1);
	free(t2);

        return *tmax > *tmin;
}

int RayCasting(float * camera, volume_t * volume, unsigned char * screen, int width, int height, float * invViewMatrix, float * invVolMatrix, float * bboxMin, float * bboxMax)
{

	int i = 0;
	int j = 0;
	int k = 0;
	int steps = 10000;
	float step  = 1.0;

	float u = 0.0;
	float v = 0.0;

	// Ray origin
	float * ro = (float*) malloc(3*sizeof(float));
	ro[0] = camera[0]; ro[1]=camera[1]; ro[2]=camera[2];
	// Ray direction
	float * rd = (float*) malloc(3*sizeof(float));
	float * r  = (float*) malloc(3*sizeof(float));

	for(i=0; i<width; i++)
	{
		u = ((float)i / (float) width) * 2.0f - 1.0f;
		for(j=0; j<height; j++)
		{
			v =  ((float)j / (float) height) * 2.0f - 1.0f;
			rd[0]=u; rd[1]=v; rd[2]=-3.0;
			normalizeVector3D(rd);
			matrixVector3D(invViewMatrix,rd,rd);
			matrixVector3D(invVolMatrix,rd,rd);	
		
			float tnear;
		        float tfar;
        		int intersected = IntersectRayAABB(ro,rd,bboxMin, bboxMax, &tnear, &tfar);
			
			screen[k] = -1; 
			screen[k+1] = -1; 
			screen[k+2] = -1; 
			screen[k+3] = 255; 
	
			if (intersected)
			{
			fprintf(stdout,"tocado\n");
				if (tnear < 0.0f)
		                {
                		        tnear = 0.0f;
                		}

		                float t = tnear;
				r[0] = ro[0]; r[1] = ro[1]; r[2] = ro[2];
					fprintf(stdout,"(%f,%f,%f)  \t",r[0] ,r[1],r[2]);
		                for (i = 0; i < steps; ++i)
                		{
					avanzar(ro, rd, t, r);
					normalizeVector3D(r);
					screen[k]   += r[0];
	                                screen[k+1] += r[1];
         	                        screen[k+2] += r[2];
					t += step;

				}	
	
				screen[k]   *= 255; 
				screen[k+1] *= 255; 
				screen[k+2] *= 255; 
			}
			else
				fprintf(stdout,"tocado\n");
			fprintf(stdout, "(%u,%u,%u)   \n",screen[k],screen[k +1],screen[k +2]);
			k+=4;
		}
	}
	
	toPNG(screen,width,height); 

	free(r);
	free(ro);
	free(rd);

	return 0;
}

int main(int argc, char ** argv)
{
	int h = 200;
	int w = 200;

	unsigned char * s 	= (unsigned char *) malloc(4*h*w*sizeof(unsigned char));
	float * camera    	= (float*) malloc(3*sizeof(float));
	float * cameratarget    = (float*) malloc(3*sizeof(float));
	float * cameraup    	= (float*) malloc(3*sizeof(float));
	float * xAxis		= (float*) malloc(3*sizeof(float));
	float * yAxis		= (float*) malloc(3*sizeof(float));
	float * zAxis		= (float*) malloc(3*sizeof(float));
	float * rotationX	= (float*) malloc(16*sizeof(float));
	float * rotationY	= (float*) malloc(16*sizeof(float));
	float * rotation	= (float*) malloc(16*sizeof(float));
	float * volumeMatrix	= (float*) malloc(16*sizeof(float));
	float * viewMatrix	= (float*) malloc(16*sizeof(float));
	float * invvolumeMatrix	= (float*) malloc(16*sizeof(float));
	float * invviewMatrix	= (float*) malloc(16*sizeof(float));
	float * bboxMax   	= (float*) malloc(3*sizeof(float));
	float * bboxMin   	= (float*) malloc(3*sizeof(float));

	volume_t volume;
	float pos[3] = {0.0, 0.0, 0.0};
	readHDF5(argv[1], "value", &volume);
	setPositionVolume(&volume, pos);
	fprintf(stdout, "Volumen leido\n");

	// Construir camera
	camera[0] = 2000.0; camera[1] = 200.0; camera[2] = 2.0;
	cameratarget[0] = 0.0; cameratarget[1] = 0.0; cameratarget[2] = 0.0;
	cameraup[0] = 0.0; cameraup[1] = 1.0; cameraup[2] = 0.0;

        float sine   = sinf(-90.0 * M_PI / 180.0f);
        float cosine = cosf(-90.0 * M_PI / 180.0f);

        rotationX[0] = 1.0f;       rotationX[1] = 0.0f;       rotationX[2] = 0.0f;       rotationX[3] = 0.0f;
        rotationX[4] = 0.0f;       rotationX[5] = cosine;     rotationX[6] = -sine;      rotationX[7] = 0.0f;
        rotationX[8] = 0.0f;       rotationX[9] = sine;       rotationX[10] = cosine;    rotationX[11] = 0.0f;
        rotationX[12] = 0.0f;      rotationX[13] = 0.0f;      rotationX[14] = 0.0f;      rotationX[15] = 1.0f;

        sine   = sinf(180.0 * M_PI / 180.0f);
        cosine = cosf(180.0 * M_PI / 180.0f);

        rotationY[0] = 1.0f;       rotationY[1] = 0.0f;       rotationY[2] = 0.0f;       rotationY[3] = 0.0f;
        rotationY[4] = 0.0f;       rotationY[5] = cosine;     rotationY[6] = -sine;      rotationY[7] = 0.0f;
        rotationY[8] = 0.0f;       rotationY[9] = sine;       rotationY[10] = cosine;    rotationY[11] = 0.0f;
        rotationY[12] = 0.0f;      rotationY[13] = 0.0f;      rotationY[14] = 0.0f;      rotationY[15] = 1.0f;

	matrixXmatrix4D(rotationX,rotationY,rotation);

	int i = 0;
	for(i=0;i<16;i++)
		volumeMatrix[i]=rotation[i];

	for(i=0;i<3;i++)
	 zAxis[i] = cameratarget[i] - camera[i];
        normalizeVector3D(zAxis);
	
	cross(zAxis,cameraup,xAxis);	
	normalizeVector3D(xAxis);

	cross(xAxis,zAxis,yAxis);
        normalizeVector3D(yAxis);

        viewMatrix[0] = xAxis[0];          viewMatrix[1] = xAxis[1];  		viewMatrix[2]  = xAxis[2];                 viewMatrix[3] = (-1.0)*dot(xAxis,camera);
        viewMatrix[4] = yAxis[0];          viewMatrix[5] = yAxis[1];  		viewMatrix[6]  = yAxis[2];                 viewMatrix[7] = (-1.0)*dot(yAxis,camera);
        viewMatrix[8] = (-1.0)*zAxis[0];   viewMatrix[9] = (-1.0)*zAxis[1]; 	viewMatrix[10] = (-1.0)*zAxis[2];          viewMatrix[11] = dot(zAxis,camera);
        viewMatrix[12] = 0.0f;             viewMatrix[13] = 0.0f;     		viewMatrix[14] = 0.0f;                     viewMatrix[15] = 1.0f;

	inverseMatrix(volumeMatrix,invvolumeMatrix);
	inverseMatrix(viewMatrix,viewMatrix);

	bboxMax[0] = 0.5;  bboxMax[1] = 0.5;  bboxMax[2] = 0.5;
	bboxMin[0] = -0.5; bboxMin[1] = -0.5; bboxMin[2] = -0.5;

	RayCasting(camera, &volume, s, w,  h, invviewMatrix, invvolumeMatrix, bboxMin, bboxMax);

	free(s);
	free(camera);
	free(cameratarget);
	free(cameraup);
	free(xAxis);
	free(yAxis);
	free(zAxis);
	free(rotation);
	free(rotationY);
	free(rotationX);
	free(volumeMatrix);
	free(viewMatrix);
	free(invvolumeMatrix);
	free(invviewMatrix);
	free(bboxMax);
	free(bboxMin);

	return 0;
}
